;;;; -*- Mode: Lisp; Package: :app.dt -*-

(in-package :app.dt)

;;; Some API Stuff

(defgeneric apply-force (force delta-time pos orientation linear-momentum angular-momentum orientation-matrix linear-velocity angular-velocity))
(defgeneric apply-torque (torque delta-time pos orientation linear-momentum angular-momentum orientation-matrix linear-velocity angular-velocity))

;;;

(defmethod mass-center ((rb rigid-body))
  (slot-value (slot-value rb 'm_w) 'b))

(defmethod (setf mass-center) (val (rb rigid-body))
  (setf (translation (slot-value rb 'm_w) ) val))

(defmethod orientation ((rb rigid-body))
  (slot-value (slot-value rb 'm_w) 'r))

(defmethod (setf orientation) (val (rb rigid-body))
  (setf (rotation (slot-value rb 'm_w)) val))

(defmethod print-object ((obj rigid-body) stream)
  (print-unreadable-object (obj stream :type t :identity t)
    (format stream "Rigid body with mass ~A and orientation ~A" (mass obj) (orientation obj))))

#| A word on the inertia tensor, or moment of inertia.  It is a 3 x 3 matrix defined as
   |  Ixx -Ixy -Ixz |
   | -Ixy  Iyy -Iyz |
   | -Ixz -Iyz  Izz |
Which is defined as the moment of inertia about a line L through origin O.  Ixx, Iyy, and Izz are the moments
of inertia about the coordinate axes, while Ixy Iyz and Ixz are the products of inertia with respect to the coordinate
axes. According to my engineering dynamics book "Engineering Mechanics, Volume 2: Dynamics" by Miriam and Kraige,
there is a "unique orientation of axes for a given origin such that the products of inertia vanish and the
moments of inertia Ixx, Iyy, Izz take on stationary values.  For this orientation, the inertia matrix takes the form"
  | Ixx   0   0 |
  |   0 Iyy     |
  |   0   0 Izz |

And goes on to give various formulas for inertia of objects in the appendix.  I will use some of those later in
the examples.
|#

(defgeneric set-physics-state (body position orientation linear-momentum angular-momentum))
(defgeneric get-physics-state (body))
(defgeneric physics-update (body time delta-time))

(defmethod initialize-instance :after ((rb rigid-body) &rest _)
  (declare (ignore _))
  (with-slots (m minv inertia inertia-inv) rb
    (setf minv (/ 1.0 m)
          inertia-inv (inverse inertia))))

(defun make-moment-tensor (ixx iyy izz ixy iyz ixz)
  (make-square-matrix     ixx (- ixy) (- ixz)
                      (- ixy)     iyy (- iyz)
                      (- ixz) (- iyz)     izz))

(defmacro with-body-state (rigid-body &body body)
  `(with-slots (m minv inertia inertia-inv p l r (v vel) w force torque) ,rigid-body
     (let ((x (mass-center ,rigid-body))
	   (q (orientation ,rigid-body)))
       ,@body
       (setf (mass-center ,rigid-body) x
	     (orientation ,rigid-body) q))))

(defmethod normalize-rotation ((body rigid-body))
  (with-slots (r) body
    (setf (orientation body) (rotation-matrix-to-quat r))))
 
(defmethod convert-state ((rb rigid-body) q p l)
  "Convert state vector from primary state (quaternion orientation, linear momentume, angular momentum)
to secondary state (matrix orientation, linear velocity, angular velocity)."
  (let* ((r (quat-to-rotation-matrix q))
         (v (m* (mass-inv rb) p))
         (w (m* r (body-inertia-inv rb) (transpose r) l)))
    (values r v w)))

;; Physics update using a runge-kutta fourth-order solver
(defmethod physics-update ((rb rigid-body) time delta-time)
  "Non-constrained motion update for a rigid body at a given time with a given time step.  Uses a fourth-order
Runge-Kutta solver."
  (with-body-state rb
    (let (a1dxdt a1dqdt a1dpdt a1dldt
          a2dxdt a2dqdt a2dpdt a2dldt
          a3dxdt a3dqdt a3dpdt a3dldt
          a4dxdt a4dqdt a4dpdt a4dldt ; Variables for the solver
          xn pn ln qn) ; The state vector
      (let* ((halfdt (* 0.5 delta-time))
             (sixthdt (/ delta-time 6.0))
             (tphalfdt (+ time halfdt))
             (tpdt (+ time delta-time)))
        ;; A1 = G(t, S0), B1 = S0 + (dt / 2) * A1
        ;; S0 = State vector at time t0
        (setq a1dxdt v
              a1dqdt (m* 0.5 (quat-mult (vector-to-quat w) q))
              a1dpdt (apply-force force time x q p l r v w)
              a1dldt (apply-torque torque time x q p l r v w)
              xn (m+ x (m* halfdt a1dxdt))
              qn (m+ q (m* halfdt a1dqdt))
              pn (m+ p (m* halfdt a1dpdt))
              ln (m+ l (m* halfdt a1dldt)))
        (multiple-value-bind (rn vn wn) (convert-state rb qn pn ln)
          ;; A2 = G(t + dt / 2, B1), B2 = S0 + (dt / 2 ) * A2
          (setq a2dxdt vn
                a2dqdt (m* 0.5 (quat-mult (vector-to-quat wn) qn))
                a2dpdt (apply-force force tphalfdt xn qn pn ln rn vn wn)
                a2dldt (apply-torque torque tphalfdt xn qn pn ln rn vn wn)
                xn (m+ x (m* halfdt a2dxdt))
                qn (m+ q (m* halfdt a2dqdt))
                pn (m+ p (m* halfdt a2dpdt))
                ln (m+ l (m* halfdt a2dldt)))
          (multiple-value-bind (rn vn wn) (convert-state rb qn pn ln)
            ;; A3 = G(t + dt / 2, B2), B3 = S0 + dt * A3
            (setq a3dxdt vn
                  a3dqdt (m* 0.5 (quat-mult (vector-to-quat wn) qn))
                  a3dpdt (apply-force force tphalfdt xn qn pn ln rn vn wn)
                  a3dldt (apply-torque torque tphalfdt xn qn pn ln rn vn wn)
                  xn (m+ x (m* delta-time a3dxdt))
                  qn (m+ q (m* delta-time a3dqdt))
                  pn (m+ p (m* delta-time a3dpdt))
                  ln (m+ l (m* delta-time a3dldt)))
            (multiple-value-bind (rn vn wn) (convert-state rb qn pn ln)
              ;; A4 = G(t +delta-time,B3), S1 = S0 + (dt / 6) * (A1 + 2 * A2 + 2 * A3 + A4)
              (setq a4dxdt vn
                    a4dqdt (m* 0.5 (quat-mult (vector-to-quat wn) qn))
                    a4dpdt (apply-force force tpdt xn qn pn ln rn vn wn)
                    a4dldt (apply-torque torque tpdt xn qn pn ln rn vn wn))
              (setf x (m+ x (m* sixthdt (m+ a1dxdt (m* 2 (m+ a2dxdt a3dxdt)) a4dxdt)))
                    q (m+ q (m* sixthdt (m+ a1dqdt (m* 2 (m+ a2dqdt a3dqdt)) a4dqdt)))
                    p (m+ p (m* sixthdt (m+ a1dpdt (m* 2 (m+ a2dpdt a3dpdt)) a4dpdt)))
                    l (m+ l (m* sixthdt (m+ a1dldt (m* 2 (m+ a2dldt a3dldt)) a4dldt))))
              (multiple-value-bind (rn vn wn) (convert-state rb q p l)  ;; Redundant?
                (setf r rn
                      v vn
                      w wn)))))))))

(defmethod update-world-data :before ((rb rigid-body) app-time)
  (physics-update rb (car app-time) (cdr app-time)))

;;; --- Making Simple Physics Objects ---

(defclass physics-mesh-constructor (mesh-constructor)
  ())

(defun physics-trimesh (vertices indices &key (colors nil) (generate-normals t) (named nil))
  "Take a list of vertices (represented as an array of vectors) and an array of indices for face triangles and
create a trimesh object."
  (let ((mesh (make-instance 'rigid-body
                             :vertices (vector->vertex-vector vertices)
                             :indices (array->gl-array indices '(unsigned-byte 32))
                             :name (or named (format nil "~A" (gensym "RIGID-BODY"))))))
    (when colors
      (setf (colors mesh) (vector->vertex-vector colors)))
    (when generate-normals
      (generate-normals mesh nil))
    mesh))

(defmethod make-geometry ((mc physics-mesh-constructor))
  (physics-trimesh (vertices mc) (indices mc) :colors (colors mc) :generate-normals (normals-p mc)))

(defun make-marble (mass size pos color)
  (let* ((izz (* (/ 2 5) mass (expt size 2.0)))
	 (pmc (make-instance 'physics-mesh-constructor))
	 (shape (standard-sphere-mesh pmc 20 20 size :color color)))
    (move-to shape (x pos) (y pos) (z pos))
    (setf (mass shape) mass
	  (body-inertia shape) (make-moment-tensor izz izz izz 0.0 0.0 0.0)
	  (elasticity shape) 0.8)
    shape))

;;; --- Examples of forces and torques ---

(defmethod apply-force ((flist t) delta-time pos o lm am om lv av)
  #(0.0 0.0 0.0))

(defmethod apply-force ((flist list) delta-time pos o lm am om lv av)
  (let ((fsum #(0.0 0.0 0.0)))
    (dolist (f flist fsum)
      (setf fsum (m+ fsum (apply-force f delta-time pos o lm am om lv av))))))

(defmethod apply-torque ((flist t) delta-time pos o lm am om lv av)
  (declare (ignore delta-time pos o lm am om lv av))
  #(0.0 0.0 0.0))

(defmethod apply-torque ((tlist list) delta-time pos o lm am om lv av)
  (let ((tsum #(0.0 0.0 0.0)))
    (dolist (torq tlist tsum)
      (setf tsum (m+ tsum (apply-torque torq delta-time pos o lm am om lv av))))))

(defclass gravity (force)
  ((accel :accessor acceleration :initarg :acceleration :initform #(0.0 -9.8 0.0))))

(defmethod apply-force ((g gravity) delta-time pos o lm am om lv av)
  (declare (ignore delta-time pos o lm am om lv av))
  (acceleration g))

;;;; ---- Computing Inertia Tensor ----
;;; Compute inertia tensor for a trimesh using techniques described in [Ebe03]

(defconstant +one-div6+ (/ 1.0 6.0))
(defconstant +one-div24+ (/ 1.0 24.0))
(defconstant +one-div60+ (/ 1.0 60.0))
(defconstant +one-div120+ (/ 1.0 120.0))

(defmethod compute-inertia-tensor ((mesh trimesh))
  (let ((integral (make-array 10))
	f1x f2x f3x g0x g1x g2x
	f1y f2y f3y g0y g1y g2y
	f1z f2z f3z g0z g1z g2z)
    (macrolet ((subexpression (w0 w1 w2 f1 f2 f3 g0 g1 g2)
		 `(let (temp0 temp1 temp2)
		    (setq temp0 (+ ,w0 ,w1)
			  ,f1 (+ temp0 ,w2)
			  temp1 (* ,w0 ,w0)
			  temp2 (+ temp1 (* ,w1 temp0))
			  ,f2 (+ temp2 (* ,w2 ,f1))
			  ,f3 (+ (* ,w0 temp1) (* ,w1 temp2) (* ,w2 ,f2))
			  ,g0 (+ ,f2 (* ,w0 (+ ,f1 ,w0)))
			  ,g1 (+ ,f2 (* ,w1 (+ ,f1 ,w1)))
			  ,g2 (+ ,f2 (* ,w2 (+ ,f1 ,w2)))))))
      (with-slots (v i) mesh
	(iter (for tidx from 0 below (triangle-count mesh))
	      (destructuring-bind (p0 p1 p2) (get-triangle mesh tidx)
		(let* ((x0 (x p0)) (y0 (y p0)) (z0 (z p0))
		       (x1 (x p1)) (y1 (y p1)) (z1 (z p1))
		       (x2 (x p2)) (y2 (y p2)) (z2 (z p2))
		       ;; Edges and cross product of edges
		       (a1 (- x1 x0)) (b1 (- y1 y0)) (c1 (- z1 z0))
		       (a2 (- x2 x0)) (b2 (- y2 y0)) (c2 (- z2 z0))
		       (d0 (- (* b1 c2) (* b2 c1)))
		       (d1 (- (* a2 c1) (* a1 c2)))
		       (d2 (- (* a1 b2) (* a2 b1))))
		  ;; Computing integral terms
		  (subexpression x0 x1 x2 f1x f2x f3x g0x g1x g2x)
		  (subexpression y0 y1 y2 f1y f2y f3y g0y g1y g2y)
		  (subexpression z0 z1 z2 f1z f2z f3z g0z g1z g2z)
		  (incf (svref integral 0) (* d0 f1x))
		  (incf (svref integral 1) (* d0 f2x))
		  (incf (svref integral 2) (* d1 f2y))
		  (incf (svref integral 3) (* d2 f2z))
		  (incf (svref integral 4) (* d0 f3x))
		  (incf (svref integral 5) (* d1 f3y))
		  (incf (svref integral 6) (* d2 f3z))
		  (incf (svref integral 7) (* d0 (+ (* y0 g0x) (* y1 g1x) (* y2 g2x))))
		  (incf (svref integral 8) (* d1 (+ (* z0 g0y) (* z1 g1y) (* z2 g2y))))
		  (incf (svref integral 9) (* d2 (+ (* x0 g0z) (* x1 g1z) (* x2 g2z)))))))
	(mult-assign (svref integral 0) +one-div6+)
	(mult-assign (svref integral 1) +one-div24+)
	(mult-assign (svref integral 2) +one-div24+)
	(mult-assign (svref integral 3) +one-div24+)
	(mult-assign (svref integral 4) +one-div60+)
	(mult-assign (svref integral 5) +one-div60+)
	(mult-assign (svref integral 6) +one-div60+)
	(mult-assign (svref integral 7) +one-div120+)
	(mult-assign (svref integral 8) +one-div120+)
	(mult-assign (svref integral 9) +one-div120+) ;; mass is integral[0]
	(let* ((mass (svref integral 0))
	       (center (vector (/ (svref integral 1) mass)
			       (/ (svref integral 2) mass)
			       (/ (svref integral 3) mass)))
	       (ixx (+ (svref integral 5) (svref integral 6)))
	       (iyy (+ (svref integral 4) (svref integral 6)))
	       (izz (+ (svref integral 4) (svref integral 5)))
	       (ixy (- (svref integral 7)))
	       (iyz (- (svref integral 8)))
	       (ixz (- (svref integral 9))))
	  (decf ixx (* mass (+ (* (y center) (y center)) (* (z center) (z center)))))
	  (decf iyy (* mass (+ (* (z center) (z center)) (* (x center) (x center)))))
	  (decf izz (* mass (+ (* (x center) (x center)) (* (y center) (y center)))))
	  (decf ixy (* mass (x center) (y center)))
	  (decf iyz (* mass (y center) (z center)))
	  (decf ixz (* mass (z center) (x center)))
	  
	  (values mass center (%matrix (list (list ixx     (- ixy) (- ixz))
					     (list (- ixy) iyy     (- iyz))
					     (list (- ixz) (- iyz) izz)))))))))
  

;;; Examples

;; Moment of a sphere:  Ixx=Iyy=Izz=2/5mr2
;; Imagining the use of mks units in the following (i.e. m = 1 = 1 kg, v = 1 = 1 m/s, etc)

(defun rb-example-1 ()
  (let* ((m 1.0)
         (r 1.0)
         (t0 0.0)
         (tn 5.0)
         (dt 0.1)
         (bod (make-marble m r #(0.0 0.0 0.0) #(1.0 1.0 1.0))))
    (pushnew (make-instance 'gravity) (force bod))
    (format t "~A~%" bod)
    (loop
      (if (> t0 tn)
        (return))
      (format t "~A: X=~A, Q=~A, V=~A, L=~A~%" t0 (mass-center bod) (orientation bod) (linear-velocity bod) (angular-velocity bod))
      (physics-update bod t0 dt)
      (incf t0 dt))))

    
;;;; Collision response

;;; Based on approach in [Eberly04], which in turn is based on [Bar01].

;;; Reduced contact sets for:
;;;  vertex-face intersections
;;;  edge-edge intersections (only when edges not parallel)
;;;  edge-face: record edge end point (vertex) if it is contained in the face, and an edge-edge
;;;             intersection point if the edge overlaps an edge on the face.
;;;  face-face: vertices of one face contained in the other face, and edge-edge intersections.
;;;
;;; N is the normal at the point of contact.
;;; At contact we assume an impulsive force F = (* f N_0) acts on the body 
;;; (and -F for the other body participating in the collision).
;;; Post-impulse linear velocity is related to pre-impulse by the equation v_post = v_pre + (/ (* f N_0) m_a).
;;; The change in linear momentum is p_post = p_pre + (* f N_0).
;;; And for angular velocity w_post = w_pre + (* (matrix-inverse J) (cross-product r (* f N_0)))
;;; Angular momentum L_post = L_pre + (cross-product r (* f N_0))
;;;
;;; r_i is defined for body i as: P - i.x, where P is the point of contact and i.x is the center of mass of the body,
;;; all values in world coordinates.
;;;
;;; f has a wicked number of terms.
;;; There is also a term f_max which limits the impulsive force represented by f
;;; such that no additional kinetic energy is imparted to either of the colliding objects.


(defstruct contact-set
  "Structure representing collision info for two objects (<body-a> and <body-b>) at time <time>.  <vertices> is the reduced set of 
contact points for the two objects.  <type> is one of
    :vertex-face
    :edge-edge
    :edge-face
    :face-face
Depending on the type of the collision the normal is unit-length and outer pointing from the contacting
face or the cross product of the contacting edge directions."
  time
  body-a
  body-b
  vertices
  normal
  type)


(defun impulsive-force-at-point (body-a body-b point normal)
  "Calculate the impulsive force, f, for two bodies (<body-a>, <body-b>) that have come into contact at point <point>.  
Note that <body-a> and <body-b> should represent the states of the two contacting bodies just prior to contact.
<normal> is a vector that is normal to the point of contact."
  (with-slots ((m_a m) (j_a inertia) (v_a v) (x_a x) (w_a w) (e_a elasticity)) body-a
    (with-slots ((m_b m) (j_b inertia) (v_b v) (x_b x) (w_b w) (e_b elasticity)) body-b
      (let* ((e (* e_a e_b))
	     (r_a (m- point x_a))
	     (r_b (m- point x_b))
	     (cross_ra_n (cross r_a normal))
	     (cross_rb_n (cross r_b normal))
	     (numerator (- (* (1+ e)
			      (+ (dot-product normal (m- v_a v_b))
				 (- (dot-product w_a cross_ra_n)
				    (dot-product w_b cross_rb_n))))))
	    (denominator (+ m_a m_b
			    (m* (transpose cross_ra_n) (inverse j_a) cross_ra_n)
			    (m* (transpose cross_rb_n) (inverse j_b) cross_rb_n))))
	(/ numerator denominator)))))
				

(defun process-colliding-contacts (contact)
  (let* ((a (contact-set-body-a contact))
	 (b (contact-set-body-b contact))
	 (e (* (elasticity a) (elasticity b))))
    ;;; Then we do something for all the vertices in the contact set?
    (case (contact-set-type contact)
      (:vertex-face (process-point-face-collision a b (contact-set-vertices contact) (contact-set-normal contact) e)))))

(defun process-point-face-collision (body-a body-b point normal e)
  (let* ((rA (m- point (mass-center body-a)))
	 (rB (m- point (mass-center body-b))))
    ))
